# -*- coding: utf-8 -*-
"""
Created on Tue Oct 23 13:55:54 2012

@author: nico
"""

#!/usr/bin/python
import cv2
import numpy as np

class artificial_horizont (object):
    def __init__ (self):
        self.error=0
        
        self.fixed_border = cv2.imread("fijo.png")
        if self.fixed_border == None:
            print "Error al cargar fijo.png"
            self.error=1
            
        self.fixed_border_mask = cv2.imread("fijo-mask.png")/255
        if self.fixed_border_mask == None:
            print "Error al cargar fijo-mask.png"
            self.error=1
            
        self.banking = cv2.imread("banqueo.png")
        if self.banking == None:
            print "Error al cargar fijo.png"
            self.error=1

        self.banking_mask = cv2.imread("banqueo-mask.png")/255
        if self.banking_mask == None:
            print "Error al cargar banqueo-mask.png"
            self.error=1

        self.horizont = cv2.imread("mundo.png")
        if self.horizont == None:
            print "Error al cargar mundo.png"
            self.error=1
            
        self.horizont_mask = cv2.imread("visible-mask.png")/255
        if self.horizont_mask == None:
            print "Error al cargar visible-mask.png"
            self.error=1
            
        if not self.error:
            self.fixed_border_h,    self.fixed_border_w     =   self.fixed_border.shape[:2]
            self.horizont_mask_h,   self.horizont_mask_w    =   self.horizont_mask.shape[:2]
            self.banking_h,         self.banking_w          =   self.banking.shape[:2]
            self.horizont_h,        self.horizont_w         =   self.horizont.shape[:2]
            
            self.horizont_full_mask =   np.zeros(self.fixed_border.shape,np.uint8)
            
            self.horizont_mask_x1=(self.fixed_border_w - self.horizont_mask_w)/2
            self.horizont_mask_x2=(self.fixed_border_w + self.horizont_mask_w)/2
            self.horizont_mask_y1=(self.fixed_border_h - self.horizont_mask_h)/2
            self.horizont_mask_y2=(self.fixed_border_h + self.horizont_mask_h)/2
            
            self.horizont_full_mask[self.horizont_mask_y1:self.horizont_mask_y2, self.horizont_mask_x1:self.horizont_mask_x2]=(1-self.horizont_mask)
            
            self.horizont_x1=(self.horizont_w - self.fixed_border_w)/2
            self.horizont_x2=(self.horizont_w + self.fixed_border_w)/2
            self.horizont_y1=(self.horizont_h - self.fixed_border_h)/2
            self.horizont_y2=(self.horizont_h + self.fixed_border_h)/2
            
            self.banking_visible=self.banking * self.banking_mask
            
            self.banking_mask_x1=(self.fixed_border_w - self.banking_w)/2
            self.banking_mask_x2=(self.fixed_border_w + self.banking_w)/2
            self.banking_mask_y1=(self.fixed_border_h - self.banking_h)/2
            self.banking_mask_y2=(self.fixed_border_h + self.banking_h)/2
            
            self.instrument=np.zeros(self.fixed_border.shape,np.uint8)

    def draw(self,pitch,roll):
        if not self.error:
            self.horizont_visible=self.horizont[self.horizont_y1-2*pitch:self.horizont_y2-2*pitch,self.horizont_x1:self.horizont_x2]
            
            
            self.horizont_visible[self.banking_mask_y1:self.banking_mask_y2,self.banking_mask_x1:self.banking_mask_x2]*=(1-self.banking_mask)
            
            self.horizont_visible[self.banking_mask_y1:self.banking_mask_y2,self.banking_mask_x1:self.banking_mask_x2]+=self.banking_visible
            self.rotation=cv2.getRotationMatrix2D((self.fixed_border_h/2,self.fixed_border_w/2), roll, 1)
            
            self.horizont_visible_rotated=cv2.warpAffine(self.horizont_visible,self.rotation,self.fixed_border.shape[:2])*(self.horizont_full_mask)*self.fixed_border_mask
            self.fixed_border_visible=self.fixed_border*(1-self.fixed_border_mask)
            
            self.instrument=self.horizont_visible_rotated+self.fixed_border_visible
        else:
            print "Hubo un error y no se puede crear el instrumento"
            self.instrument=np.zeros([10,10],np.uint8)
            
            
#fijoRGB_mask=cv2.merge([fijo_mask,fijo_mask,fijo_mask])/255
#roi = cv2.multiply(visible,mundo[0:fijo.shape[0], 0:fijo.shape[1]])
#image = cv2.cv.CreateImage(fijo.size, cv.CV_8U,4)
#todo=cv2.add(fijo,roi)

if __name__ == '__main__':
    my_horizont=artificial_horizont()
    my_horizont.draw(30,20)
    while True:
        #img = cv.QueryFrame(capture)
        #
        c=cv2.waitKey(20)
        #cv.Circle(image, (image.width/2,image.height/2), 50, cv.Scalar(0,255,255),2)
        #cv.Line(image,(image.width/2-50,image.height/2),(image.width/2+50,image.height/2),cv.Scalar(0,255,255))
        cv2.imshow("todo",my_horizont.instrument )
        if (c & 255) == 27:
            break
